#include "navigation.h"

Navigation* Navigation::m_handel = nullptr;

Navigation* Navigation::Get()
{
        static std::once_flag once;
        std::call_once(once, [&](){ m_handel = new Navigation; });
        return m_handel;
}

void Navigation::MoveForward(float length)
{
    Navigation* nav = Get();
    NavPoint StartPoint;
    nav->nowPosion >> StartPoint;
    bool doublecount = 0;
    while(1)
    {
        geometry_msgs::Twist twist;
        NavPoint nowpoint;
        nav->nowPosion >> nowpoint;
        float deltax = abs(nowpoint.x - StartPoint.x);
        float deltay = abs(nowpoint.y - StartPoint.y);
        float distance = sqrt(deltax * deltax + deltay * deltay) + 0.01;
        distance = distance;
        //std::cout << "distance = " << distance << std::endl;
        if(length <= distance)
        {
            twist.linear.x = 0;
            publisher.publish(twist);
            return;
        }
        else
        {
            twist.linear.x = 0.2;
            publisher.publish(twist);
            doublecount = 0;
        }
    }
}

void Navigation::MoveBackward(float length)
{
    Navigation* nav = Get();
    NavPoint StartPoint;
    nav->nowPosion >> StartPoint;
    bool doublecount = 0;
    while(1)
    {
        geometry_msgs::Twist twist;
        NavPoint nowpoint;
        nav->nowPosion >> nowpoint;
        float deltax = abs(nowpoint.x - StartPoint.x);
        float deltay = abs(nowpoint.y - StartPoint.y);
        float distance = sqrt(deltax * deltax + deltay * deltay) + 0.01;
        //std::cout << "distance = " << distance << std::endl;
        if(length <= distance)
        {
            twist.linear.x = 0;
            publisher.publish(twist);
            publisher.publish(twist);
            return;
        }
        else
        {
            twist.linear.x = -0.2;
            publisher.publish(twist);
            doublecount = 0;
        }
    }
}

void Navigation::TurnRight(float angle)
{
    Navigation* nav = Get();
    float StartAngle = 0;
    nav->towards >> StartAngle;
    while(1)
    {
        geometry_msgs::Twist twist;
        float nowangle;
        nav->towards >> nowangle;
        float anglediff = abs(nowangle - StartAngle) + 3;
        if(anglediff > 270)
        {
            anglediff = abs(anglediff - 360);
        }
        if(anglediff > angle)
        {
            twist.linear.x = -0.2;
            twist.angular.z = 5;
            publisher.publish(twist);
            delayMilliseconds(500); 
            twist.linear.x = 0;
            twist.angular.z = 0;
            publisher.publish(twist);
            publisher.publish(twist);
            return;
        }
        else
        {
            twist.linear.x = 0.2;
            twist.angular.z = -5;
            publisher.publish(twist);
        }
    }
}

void Navigation::TurnLeft(float angle)
{
    Navigation* nav = Get();
    float StartAngle = 0;
    nav->towards >> StartAngle;
    while(1)
    {
        geometry_msgs::Twist twist;
        float nowangle;
        nav->towards >> nowangle;
        float anglediff = abs(nowangle - StartAngle) + 3;
        //std::cout << "o_angle_diff == " << anglediff << std::endl;
        if(anglediff > 270)
        {
            anglediff = abs(anglediff - 360);
        }
        //std::cout << "angle_diff == " << anglediff << std::endl;
        if(anglediff > angle)
        {
            twist.linear.x = -0.2;
            twist.angular.z = -5;
            publisher.publish(twist);
            delayMilliseconds(500); 
            twist.linear.x = 0;
            twist.angular.z = 0;
            publisher.publish(twist);
            publisher.publish(twist);
            return;
        }
        else
        {
            twist.linear.x = 0.2;
            twist.angular.z = 5;
            publisher.publish(twist);
        }
    }
}

void Navigation::init(ros::Publisher _publisher)
{
    publisher = _publisher;
    NavPoint pt;
    pt.x = 0;
    pt.y = 0;
    float w = 0;
    Get()->nowPosion << pt;
    Get()->towards << w; 
}

void Navigation::UpdatePosition(const tf2_msgs::TFMessage::ConstPtr& msg)
{
    Navigation* nav = Get();
    NavPoint pt;
    pt.x = msg->transforms[0].transform.translation.x;
    pt.y = msg->transforms[0].transform.translation.y;
    float toward = GetToward(msg->transforms[0].transform.rotation. x,msg->transforms[0].transform.rotation.y, msg->transforms[0].transform.rotation.z, msg->transforms[0].transform.rotation.w);
    nav->nowPosion << pt;
    nav->towards << toward;
    //std::cout << "toward = " << toward << std::endl;
}

Navigation::Navigation()
{
}

void Navigation::BackRight(float angle)
{
    Navigation* nav = Get();
    float StartAngle = 0;
    nav->towards >> StartAngle;
    while(1)
    {
        geometry_msgs::Twist twist;
        float nowangle;
        nav->towards >> nowangle;
        float anglediff = abs(nowangle - StartAngle) + 3;
        if(anglediff > 270)
        {
            anglediff = abs(anglediff - 360);
        }
        if(anglediff > angle)
        {
            twist.linear.x = 0.2;
            twist.angular.z = -5;
            publisher.publish(twist);
            delayMilliseconds(500); 
            twist.linear.x = 0;
            twist.angular.z = 0;
            publisher.publish(twist);
            publisher.publish(twist);
            return;
        }
        else
        {
            twist.linear.x = -0.2;
            twist.angular.z = 5;
            publisher.publish(twist);
        }
    }
}

void Navigation::BackLeft(float angle)
{
    Navigation* nav = Get();
    float StartAngle = 0;
    nav->towards >> StartAngle;
    while(1)
    {
        geometry_msgs::Twist twist;
        float nowangle;
        nav->towards >> nowangle;
        float anglediff = abs(nowangle - StartAngle) + 3;
        //std::cout << "o_angle_diff == " << anglediff << std::endl;
        if(anglediff > 270)
        {
            anglediff = abs(anglediff - 360);
        }
        //std::cout << "angle_diff == " << anglediff << std::endl;
        if(anglediff > angle)
        {
            twist.linear.x = 0.2;
            twist.angular.z = 5;
            publisher.publish(twist);
            delayMilliseconds(500); 
            twist.linear.x = 0;
            twist.angular.z = 0;
            publisher.publish(twist);
            publisher.publish(twist);
            return;
        }
        else
        {
            twist.linear.x = -0.2;
            twist.angular.z = -5;
            publisher.publish(twist);
        }
    }
}

void delayMilliseconds(int milliseconds)
{
    usleep(milliseconds * 1000);  // 将毫秒转换为微秒并延时
}



float GetToward(float x, float y, float z, float w) 
{
    // 计算四元数的角度
    float angle = 2.0f * std::acos(w);

    // 计算平面朝向（yaw）
    float toward = std::atan2(2.0f * (x * y + z * w), 1.0f - 2.0f * (y * y + z * z));

    // 将朝向限制在 [0, 2π) 范围内
    if (toward < 0.0f) 
    {
        toward += 2.0f * M_PI;
    }

    toward = toward / M_PI * 180;

    return toward;
}